
#include <BShape>
#include <BRestraint>
#include <BBody>
#include "member_BBody.h"

using namespace BWE;

#define member					(*(member_BBody*)_ptr)
#define member_allocate()		_ptr = new member_BBody(this)
#define member_release()		delete (member_BBody*)_ptr

BBody::BBody()
{
	member_allocate();
}
BBody::BBody(const BString& name) : BNode(name)
{
	member_allocate();
}
BBody::~BBody()
{
	member_release();
}

void BBody::copy(const BNode* other)
{
	BNode::copy(other);

	if (const BBody* body = dynamic_cast<const BBody*>(other))
	{
		member.state = body_member(body)->state;
		member.kinematic = body_member(body)->kinematic;
		member.continuous = body_member(body)->continuous;

		member.mass = body_member(body)->mass;
		member.gravity = body_member(body)->gravity;
		member.restitution = body_member(body)->restitution;
		member.friction = body_member(body)->friction;
		member.rollingFriction = body_member(body)->rollingFriction;

		member.force = body_member(body)->force;
		member.torque = body_member(body)->torque;
		member.pushVelocity = body_member(body)->pushVelocity;
		member.turnVelocity = body_member(body)->turnVelocity;
		member.linearVelocity = body_member(body)->linearVelocity;
		member.angularVelocity = body_member(body)->angularVelocity;

		member.deactivatedTime = body_member(body)->deactivatedTime;
		member.linearThreshold = body_member(body)->linearThreshold;
		member.angularThreshold = body_member(body)->angularThreshold;

		member.dirty = body_member(body)->dirty;
		member.invMass = body_member(body)->invMass;
		member.inertia = body_member(body)->inertia;
		member.invInertia = body_member(body)->invInertia;

	}
}
BNode* BBody::clone() const
{
	BBody* body = new BBody();
	body->copy(this);
	return body;
}

bool BBody::insertShape(BShape* shape)
{
	if (BNode::insertShape(shape))
	{
		member.dirty = true;
		return true;
	}
	return false;
}
bool BBody::removeShape(BShape* shape)
{
	if (BNode::removeShape(shape))
	{
		member.dirty = true;
		return true;
	}
	return false;
}
bool BBody::clearShapes()
{
	if (BNode::clearShapes())
	{
		member.dirty = true;
		return true;
	}
	return false;
}

bool BBody::setState(BBody::State state) const
{
	if (member.state != state)
	{
		member.state = state;
		if (state == BBody::State_Active)
			member.deactivatingTime = 0;
		this->emit(Signal_StateChanged, member.state);
		return true;
	}
	return false;
}
BBody::State BBody::state() const
{
	return member.state;
}

void BBody::setDeactivatedTime(BReal time)
{
	member.deactivatedTime = time;
}
BReal BBody::deactivatedTime() const
{
	return member.deactivatedTime;
}

void BBody::setContinuous(bool continuous)
{
	member.continuous = continuous;
}
bool BBody::continuous() const
{
	return member.continuous;
}

bool BBody::setKinematic(bool kinematic)
{
	if (member.kinematic != kinematic)
	{
		member.kinematic = kinematic;
		this->emit(Signal_Changed);
		return true;
	}
	return false;
}
bool BBody::kinematic() const
{
	return member.kinematic;
}

void BBody::activate() const
{
	if (member.mass <= REAL_EPSILON)
		return;
	if (this->setState(State_Active))
	{
		member.deactivatingTime = 0;
	}
}

void BBody::setMass(BReal mass)
{
	if (member.mass != mass)
	{
		member.mass = mass;
		member.dirty = true;
		this->emit(Signal_MassChanged, mass);
	}
}
BReal BBody::mass() const
{
	return member.mass;
}

void BBody::setInertia(BReal ix, BReal iy, BReal iz)
{
	setInertia(BVector(ix, iy, iz));
}
void BBody::setInertia(const BVector& inertia)
{
	if (member.inertia != inertia)
	{
		member.inertia = inertia;
		member.invInertia.x() = inertia.x() != 0 ? 1 / inertia.x() : 0;
		member.invInertia.y() = inertia.y() != 0 ? 1 / inertia.y() : 0;
		member.invInertia.z() = inertia.z() != 0 ? 1 / inertia.z() : 0;
		if (this->checks(Signal_InertiaChanged))
			this->emit(Signal_InertiaChanged, inertia);
	}
}
const BVector& BBody::inertia() const
{
	return member.inertia;
}

void BBody::setGravity(BReal gx, BReal gy, BReal gz)
{
	setGravity(BVector(gx, gy, gz));
}
void BBody::setGravity(const BVector& gravity)
{
	if (member.gravity != gravity)
	{
		member.gravity = gravity;
		if (this->checks(Signal_GravityChanged))
			this->emit(Signal_GravityChanged, gravity);
	}
}
const BVector& BBody::gravity() const
{
	return member.gravity;
}

void BBody::setRestitution(BReal restitution)
{
	if (member.restitution != restitution)
	{
		member.restitution = restitution;
		this->emit(Signal_Restitution_Changed, restitution);
	}
}
BReal BBody::restitution() const
{
	return member.restitution;
}

void BBody::setFriction(BReal friction)
{
	if (member.friction != friction)
	{
		member.friction = friction;
		this->emit(Signal_FrictionChanged, friction);
	}
}
BReal BBody::friction() const
{
	return member.friction;
}

void BBody::setRollingFriction(BReal rollingFriction)
{
	if (member.rollingFriction != rollingFriction)
	{
		member.rollingFriction = rollingFriction;
		this->emit(Signal_FrictionChanged, rollingFriction);
	}
}
BReal BBody::rollingFriction() const
{
	return member.rollingFriction;
}

void BBody::setForce(BReal fx, BReal fy, BReal fz)
{
	setForce(BVector(fx, fy, fz));
}
void BBody::setForce(const BVector& force)
{
	if (member.force != force)
	{
		member.force = force;
		if (this->checks(Signal_ForceChanged))
			this->emit(Signal_ForceChanged, force);
	}
}
const BVector& BBody::force() const
{
	return member.force;
}

void BBody::setTorque(BReal tx, BReal ty, BReal tz)
{
	setTorque(BVector(tx, ty, tz));
}
void BBody::setTorque(const BVector& torque)
{
	if (member.torque != torque)
	{
		member.torque = torque;
		if (this->checks(Signal_TorqueChanged))
			this->emit(Signal_TorqueChanged, torque);
	}
}
const BVector& BBody::torque() const
{
	return member.torque;
}

void BBody::setLinearVelocity(const BVector& linearVelocity)
{
	if (member.linearVelocity != linearVelocity)
	{
		member.linearVelocity = linearVelocity;
		if (this->checks(Signal_VelocityChanged))
			this->emit(Signal_VelocityChanged, linearVelocity);
	}
}
const BVector& BBody::linearVelocity() const
{
	return member.linearVelocity;
}

void BBody::setAngularVelocity(const BVector& angularVelocity)
{
	if (member.angularVelocity != angularVelocity)
	{
		member.angularVelocity = angularVelocity;
		if (this->checks(Signal_VelocityChanged))
			this->emit(Signal_VelocityChanged, angularVelocity);
	}
}
const BVector& BBody::angularVelocity() const
{
	return member.angularVelocity;
}

void BBody::setLinearThreshold(BReal linearThreshold)
{
	if (member.linearThreshold != linearThreshold)
	{
		member.linearThreshold = linearThreshold;
		this->emit(Signal_Changed);
	}
}
BReal BBody::linearThreshold() const
{
	return member.linearThreshold;
}

void BBody::setAngularThreshold(BReal angularThreshold)
{
	if (member.angularThreshold != angularThreshold)
	{
		member.angularThreshold = angularThreshold;
		this->emit(Signal_Changed);
	}
}
BReal BBody::angularThreshold() const
{
	return member.angularThreshold;
}

BVector BBody::totalForce() const
{
	return member.gravity * member.mass + member.force;
}
BVector BBody::totalTorque() const
{
	return member.torque;
}
BVector BBody::velocityInPoint(const BVector& point) const
{
	return member.linearVelocity + member.angularVelocity.cross(point);
}

void BBody::applyImpulse(const BVector& impulse, const BVector& point)
{
	if (member.invMass != 0)
	{
		member.linearVelocity += impulse * member.invMass;
		BVector torque = point.cross(impulse);
		member.angularVelocity += member.angularComponent(torque);
	}
}
void BBody::applyPushImpulse(const BVector& impulse, const BVector& point)
{
	if (member.invMass != 0)
	{
		member.pushVelocity += impulse * member.invMass;
		BVector torque = point.cross(impulse);
		member.turnVelocity += member.angularComponent(torque);
	}
}

void BBody::applyCentralImpulse(const BVector& impulse)
{
	member.linearVelocity += impulse * member.invMass;
}
void BBody::applyTorqueImpulse(const BVector& torque)
{
	member.angularVelocity += member.angularComponent(torque);
}

void BBody::applyCentralPushImpulse(const BVector& impulse)
{
	member.pushVelocity += impulse * member.invMass;
}
void BBody::applyTorqueTurnImpulse(const BVector& torque)
{
	member.turnVelocity += member.angularComponent(torque);
}

bool BBody::restrained(BRestraint* restraint)
{
	return member.restraints.contain(restraint);
}

int BBody::restraintCount() const
{
	return member.restraints.size();
}
BRestraint* BBody::restraint(int index)
{
	return member.restraints(index);
}
const BRestraint* BBody::restraint(int index) const
{
	return member.restraints(index);
}

BRestraint* BBody::restraintWith(const BBody* other)
{
	if (!other)
		return 0;
	for (int i = 0; i < member.restraints.size(); i++)
	{
		BRestraint* restraint = member.restraints[i];
		if (restraint->contain(other))
			return restraint;
	}
	return 0;
}
const BRestraint* BBody::restraintWith(const BBody* other) const
{
	if (!other)
		return 0;
	for (int i = 0; i < member.restraints.size(); i++)
	{
		const BRestraint* restraint = member.restraints[i];
		if (restraint->contain(other))
			return restraint;
	}
	return 0;
}

BRestraint* BBody::namedRestraint(const BString& name)
{
	if (name.empty())
		return 0;
	for (int i = 0; i < member.restraints.size(); i++)
	{
		BRestraint* restraint = member.restraints[i];
		if (restraint->name() == name)
			return restraint;
	}
	return 0;
}
const BRestraint* BBody::namedRestraint(const BString& name) const
{
	if (name.empty())
		return 0;
	for (int i = 0; i < member.restraints.size(); i++)
	{
		const BRestraint* restraint = member.restraints[i];
		if (restraint->name() == name)
			return restraint;
	}
	return 0;
}

BSpace BBody::space(const BMatrix& matrix) const
{
	const BBox& box = this->box();
	BSpace space(matrix * box.center());
	space.expand(matrix * box.v0());
	space.expand(matrix * box.v1());
	space.expand(matrix * box.v2());
	space.expand(matrix * box.v3());
	space.expand(matrix * box.v4());
	space.expand(matrix * box.v5());
	space.expand(matrix * box.v6());
	space.expand(matrix * box.v7());
	return space;
}

bool BBody::query(const BString& text, BValue& value) const
{
	if (text == "push-velocity")
	{
		value = member.solverData.pushVelocity;
		return true;
	}
	if (text == "turn-velocity")
	{
		value = member.solverData.turnVelocity;
		return true;
	}

	return BNode::query(text, value);
}

bool BBody::contact(BContact& contact)
{
	return true;
}
